//------------------------------------------------------------------------------
// Wander Service for Boe-Bot
//
// IMPORTANT NOTE!
// Uncomment ONLY ONE of the definitions below for your robot!
// You will get compilation errors if you define more than one.
//
// This solution is primarily intended for the Boe-Bot. However,
// if uses generic contracts and can also be made to work with
// the LEGO NXT using the original services shipped with MRDS V1.5.
// Also, if you recompile for the LEGO NXT then you will probably
// want to change the Project properties so that it will use the
// correct manifest which is Wander.LEGO.manifest.xml instead of
// Wander.BoeBot.manifest.xml.
//
#define BOEBOT
//#define LEGONXT
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using wanderboebot = ProMRDS.Robotics.WanderBoeBot;
// Add a reference to a generic Bumper (in Robotics.Common.Proxy)
using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;
// Add a reference to the Differential Drive
using drive = Microsoft.Robotics.Services.Drive.Proxy;
// Required for Fault class
using W3C.Soap;

#if LEGONXT
using sonar = Microsoft.Robotics.Services.Sonar.Proxy;
#endif


namespace ProMRDS.Robotics.WanderBoeBot
{
    
    /// <summary>
    /// Implementation class for WanderBoeBot
    /// </summary>
    [DisplayName("WanderBoeBot")]
    [Description("The Wander Service for the Boe-Bot")]
    [Contract(Contract.Identifier)]
    public class WanderBoeBotService : DsspServiceBase
    {
        #region constants

#if BOEBOT
        const string _platform = "Boe-Bot";
#endif
#if LEGONXT
        const string _platform = "LEGO NXT";
#endif
        const float drivePower = 0.5f;  // Speed driving forward
        const float rotatePower = 0.25f; // Speed during rotation
        const int driveTime = 1200;     // Time to drive forward (millisec)
        const int rotateTime = 920;     // Time to rotate

        const float backupPower = 0.5f; // Power while backing up
        const int backupTime = 1000;    // To to back up on physical contact

        #endregion

        /// <summary>
        /// _state
        /// </summary>
        private WanderBoeBotState _state = new WanderBoeBotState();

        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/wanderboebot", AllowMultipleInstances=false)]
        private WanderBoeBotOperations _mainPort = new WanderBoeBotOperations();

        #region partners

        // Partner with a Bumper (Contact Sensor) service
        [Partner("bumper", Contract = bumper.Contract.Identifier,
             CreationPolicy = PartnerCreationPolicy.UseExisting)]
        // Create a port to send requests
        private bumper.ContactSensorArrayOperations _bumperPort = new bumper.ContactSensorArrayOperations();
        // Create the bumper notification port
        private bumper.ContactSensorArrayOperations _bumperNotificationPort = new bumper.ContactSensorArrayOperations();

#if LEGONXT
        [Partner("sonar", Contract = sonar.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExisting)]
        private sonar.SonarOperations _sonarPort = new sonar.SonarOperations();
#endif

        // Partner with a Differential Drive service
        [Partner("drive", Contract = drive.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExisting)]
        // Create a port to talk to the diff drive
        drive.DriveOperations _drivePort = new drive.DriveOperations();

        #endregion

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public WanderBoeBotService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
			base.Start();

			// Add service specific initialization here.

            // Tell the user which version of the service this is
            LogInfo(LogGroups.Console, "WANDER - Built for " + _platform);

            // Allocate the necessary storage for the bumper state
            // The size of this array will depend on the particular robot
            _state.bumperStates = new bool[4];
            _state.bumperNames = new string[4];
            for (int i = 0; i < 4; i++)
            {
                _state.bumperStates[i] = false;
                _state.bumperNames[i] = null;
            }

            // Now subscribe to bumper notifications
            SpawnIterator(SubscribeToBumpers);

            SpawnIterator(Countdown);

            /*
            // Code to explicitly get a sonar range
            yield return Activate(Arbiter.Choice(
                _sonarPort.Get(),
                delegate(sonar.SonarState s)
                {
                    // Do something with the sonar reading here
                    LogInfo(s.DistanceMeasurement);
                },
                delegate(Fault f)
                {
                    // Something went wrong!
                }
            ));
             */
        }

        IEnumerator<ITask> Countdown()
        {
            // Wait for the robot to initialize, otherwise it will
            // miss the initial command
            for (int i = 10; i > 0; i--)
            {
                LogInfo(LogGroups.Console, i.ToString());
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(1000),
                    delegate(DateTime timeout) { });
            }

            LogInfo(LogGroups.Console, "Starting now ...");

            // Make sure that the drive is enabled first!
            _drivePort.EnableDrive(true);

            // Start the robot on its way!
            _drivePort.SetDrivePower(drivePower, drivePower);

        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }


        #region bumpers
        /// <summary>
        /// Subscribe to the Bumpers service
        /// </summary>
        IEnumerator<ITask> SubscribeToBumpers()
        {
            // Get the contact sensor info
            bumper.Get GetRequest = new bumper.Get();
            _bumperPort.Post(GetRequest);
            yield return Arbiter.Choice(
                GetRequest.ResponsePort,
                BumperInfo,
                delegate(Fault fault) { }
            );

            // Subscribe to the bumper service
            // Receive notifications on the bumperNotificationPort
            _bumperPort.Subscribe(_bumperNotificationPort);

            // Start listening for updates from the bumper service
            // Note that we use the Exclusive Receiver Group so that
            // two bumper messages cannot be processed at the same time
            Activate(
                Arbiter.Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<bumper.Update>
                            (true, _bumperNotificationPort, BumperHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );

        }

        void BumperInfo(bumper.ContactSensorArrayState s)
        {
            int count = s.Sensors.Count;
            int i;
            string message;
            for (i = 0; i < count; i++)
            {
                message = s.Sensors[i].HardwareIdentifier.ToString() + ": " + s.Sensors[i].Name;
                LogInfo(LogGroups.Console, message);
            }
        }

#if BOEBOT
        private enum BumperIDs { IR_LEFT = 0, IR_RIGHT, WHISKER_LEFT, WHISKER_RIGHT };
#endif

        /// <summary>
        /// Handle Bumper Notifications
        /// </summary>
        /// <param name="notification"></param>
        IEnumerator<ITask> BumperHandler(bumper.Update notification)
        {
            string message;
            string bumperName;
            DateTime thisTimestamp;
            TimeSpan timediff;

            // Find out which bumper this is
            // BoeBot numbers from 1 to 4
            // LEGO NXT numbers from 1 to 4
            int num = notification.Body.HardwareIdentifier - 1;

            if (string.IsNullOrEmpty(notification.Body.Name))
                bumperName = "NO NAME";
            else
                bumperName = notification.Body.Name.ToLowerInvariant();

            if (!notification.Body.Pressed)
            {
                message = "Bumper " + num.ToString() + " (" + bumperName + ") was released.";
                LogInfo(LogGroups.Console, message);
            }
            else
            {
                message = "Bumper " + num.ToString() + " (" + bumperName + ") was pressed.";
                LogInfo(LogGroups.Console, message);
            }

            if (num >= 0 && num < _state.bumperStates.Length)
            {
#if LEGONXT
                if (bumperName.Contains("touchsensor") || bumperName.Contains("ultrasonicsensor.far"))
#endif
                {
                    _state.bumperStates[num] = notification.Body.Pressed;
                    if (_state.bumperNames[num] == null)
                        _state.bumperNames[num] = bumperName;
                }
            }
            else
            {
                message = "Invalid Hardware ID: " + num.ToString();
                LogInfo(LogGroups.Console, message);
            }

#if BOEBOT
            // Handle the physical touch sensors first because these are the
            // most important
            if (_state.bumperStates[(int)BumperIDs.WHISKER_LEFT] || _state.bumperStates[(int)BumperIDs.WHISKER_RIGHT])
            {
                // Stop immediately!
                //_drivePort.SetDrivePower(0, 0);

                // Back up to get away from the obstacle
                _drivePort.SetDrivePower(-backupPower, -backupPower);
                LogInfo(LogGroups.Console, ">>> Backing Up");
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(backupTime),
                    delegate(DateTime timeout) { });
                _drivePort.SetDrivePower(0, 0);

                if (_state.bumperStates[(int)BumperIDs.WHISKER_LEFT])
                {
                    // Turn right
                    LogInfo(LogGroups.Console, ">>> Turn Right");
                    _drivePort.SetDrivePower(rotatePower, -rotatePower);
                }
                else
                {
                    // Turn left
                    LogInfo(LogGroups.Console, ">>> Turn Left");
                    _drivePort.SetDrivePower(-rotatePower, rotatePower);
                }

                // Wait a while
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(rotateTime),
                    delegate(DateTime timeout) { });

                // Stop the motors and wait for robot to settle
                _drivePort.SetDrivePower(0, 0);

                // Finally, skip all of the bumper messages that occurred
                // while we were taking evasive action
                SkipBumpNotifications();
            }

            // Now that everything has settled down
            if (_state.bumperStates[(int)BumperIDs.IR_LEFT] && _state.bumperStates[(int)BumperIDs.IR_RIGHT])
            {
                // Both IRs see something so back up
                // Eventually one of them must lose sight of the obstacle
                LogInfo(LogGroups.Console, ">>> Reverse");
                _drivePort.SetDrivePower(-drivePower, -drivePower);
            }
            else if (_state.bumperStates[(int)BumperIDs.IR_LEFT] && !_state.bumperStates[(int)BumperIDs.IR_RIGHT])
            {
                // Obstacle on the left, so turn right until gone
                LogInfo(LogGroups.Console, ">>> Veer Right");
                _drivePort.SetDrivePower(rotatePower, -rotatePower);
            }
            else if (!_state.bumperStates[(int)BumperIDs.IR_LEFT] && _state.bumperStates[(int)BumperIDs.IR_RIGHT])
            {
                // Obstacle on the right, so turn left until gone
                LogInfo(LogGroups.Console, ">>> Veer Left");
                _drivePort.SetDrivePower(-rotatePower, rotatePower);
            }
            else
            {
                // No obstacles so drive forwards
                LogInfo(LogGroups.Console, ">>> Forward");
                _drivePort.SetDrivePower(drivePower, drivePower);
            }
#endif

#if LEGONXT
            // Handle the physical touch sensor first because it is the
            // most important
            if (bumperName.Contains("touchsensor") && notification.Body.Pressed)
            {
                // Back up to get away from the obstacle
                _drivePort.SetDrivePower(-backupPower, -backupPower);
                LogInfo(LogGroups.Console, ">>> Backing Up");
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(backupTime),
                    delegate(DateTime timeout) { });
                _drivePort.SetDrivePower(0, 0);

                // Turn left
                LogInfo(LogGroups.Console, ">>> Turn Left");
                _drivePort.SetDrivePower(-rotatePower, rotatePower);

                // Wait a while
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(rotateTime),
                    delegate(DateTime timeout) { });

                // Stop the motors and wait for robot to settle
                _drivePort.SetDrivePower(0, 0);

                // Finally, skip all of the bumper messages that occurred
                // while we were taking evasive action
                SkipBumpNotifications();
            }

            // Now check the Ultrasonic sensor
            // NOTE: There are two possible "bumpers" for an Ultrasonic sensor -
            // near and far. Unfortunately, they do not always release.
            bool veer = false;
            for (int i = 0; i < _state.bumperStates.Length; i++)
            {
                if (_state.bumperNames[i] != null && _state.bumperNames[i].Contains("ultrasonicsensor.far") && _state.bumperStates[i])
                {
                    veer = true;
                    break;
                }
            }
            if (veer)
            {
                //LogInfo(LogGroups.Console, ">>> Veer Left");
                //_drivePort.SetDrivePower(-rotateSpeed / 2, rotateSpeed);
                // Turn left
                LogInfo(LogGroups.Console, ">>> Turn Right");
                _drivePort.SetDrivePower(rotatePower, -rotatePower);

                // Wait a while
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(rotateTime),
                    delegate(DateTime timeout) { });

                // Stop the motors and wait for robot to settle
                _drivePort.SetDrivePower(0, 0);

                // Finally, skip all of the bumper messages that occurred
                // while we were taking evasive action
                SkipBumpNotifications();
            }
            else
            {
                LogInfo(LogGroups.Console, ">>> Forward");
                _drivePort.SetDrivePower(drivePower, drivePower);

            }      
#endif

            /*
            // OK, we have a bump!
            // What if it is the OPPOSITE bumper?
            // This should be handled better.
            // In particular, there are instances where both front
            // AND back bumpers are triggered! It would be a better
            // idea to remember the direction of motion and always
            // back up when a bumper notification is received.

            // Get the current time
            thisTimestamp = System.DateTime.Now;
            // Make sure that we have waited long enough for the last
            // motion to complete. This is a fudge that effectively
            // renders the robot blind for a short period of time.
            // Not really a good solution if there is a chance that
            // the opposite bumper will be triggered legitimately.
            // It is sometimes referred to as a "dead zone" and is
            // similar to what is used in simple "bang bang" or
            // "on/off" control to avoid the continuous oscillations.
            if (thisTimestamp < _state.nextTimestamp)
            {
                int count = 0;
                bumper.Replace testBump = null;
                bumper.Replace testBump2 = null;
                int skip = bumperNotificationPort.P3.ItemCount;
                // We don't want any more bump notifications while we
                // are processing the current one.
                // This code for skipping notifications uses a pattern
                // from the Traxster motor.cs. However, the types of
                // messages involved are different so it is not just a
                // copy and paste :-(
                // Note that the queue can easily get over a thousand
                // messages in a short space of time!
                for (int i = 0; i < skip; i++)
                {
                    testBump = (bumper.Replace)bumperNotificationPort.P3.Test();
                    if (testBump != null)
                    {
                        count++;
                        // reply and pretend everything went well
                        testBump.ResponsePort.Post(DefaultReplaceResponseType.Instance);
                    }
                }
                if (count > 0)
                {
                    message = "Ignored " + num.ToString() + " - " + count.ToString() + " messages";
                    //                    LogInfo(LogGroups.Console, message);
                }
                // Return without doing anything
                return;
            }

            // Wait for the motion to complete for this amount
            // of time, i.e. ignore future bumps!!!
            // This is a fudge because we don't know for sure
            // how long any particular motion will take, but
            // it's the best we can do.
            _state.nextTimestamp = thisTimestamp.AddMilliseconds(3500);

            // From Robotics Tutorial 3
            // Look carefully at the code. At first glance, it seems
            // to call the same function for either bumper, but the
            // polarity parameter is different.
            if (bumperName.Contains("front"))
            {
                if (_state.insideBehavior <= 0)
                {
                    LogInfo(LogGroups.Console, ">>> Start -1");
                    // Set a flag so that multiple threads
                    // will not execute the behavior at the
                    // same time. This is possible, but the
                    // motor commands will get "interleaved"
                    // and the robot does some really strange
                    // things!
                    _state.insideBehavior++;
                    // Create a new iterator to execute the
                    // behavior
                    SpawnIterator<double>(-1, BackUpTurnAndMove);
                }
                return;
            }

            if (bumperName.Contains("rear"))
            {
                if (_state.insideBehavior <= 0)
                {
                    LogInfo(LogGroups.Console, ">>> Start +1");
                    // Set the synchronization flag
                    _state.insideBehavior++;
                    SpawnIterator<double>(1, BackUpTurnAndMove);
                }
                return;
            }
             */

        }

        // Skip over bumper notifications that have accumulated while
        // we were busy. This is important because we only receive
        // changes in the state and need to keep track of what is
        // happening.
        void SkipBumpNotifications()
        {
            bumper.Update msg;
            int num;
            int count = 0;
            string name;
            while ((msg = _bumperNotificationPort.Test<bumper.Update>()) != null)
            {
                num = msg.Body.HardwareIdentifier - 1;
                if (num >= 0 && num < _state.bumperStates.Length)
                {
                    name = msg.Body.Name.ToLowerInvariant();
                    if (name.Contains("touchsensor") || name.Contains("ultrasonicsensor.far"))
                    {
                        _state.bumperStates[num] = msg.Body.Pressed;
                        if (_state.bumperNames[num] == null)
                            _state.bumperNames[num] = name;
                    }
                }
                count++;
            }
            if (count > 0)
                LogInfo(LogGroups.Console, "Skipped " + count.ToString() + " messages");
        }

        #endregion

    }
}
